//============================================================================
//	Johns Hopkins University Engineering for Professionals
//	605.467 Computer Graphics and 605.767 Applied Computer Graphics
//	Instructor:	David W. Nesbitt

//	Author:  David W. Nesbitt
//	File:    Matrix.h
//	Purpose: Matrix class
//          Student should include "geometry.h" to get all 
//          class definitions included in proper order.
//
//============================================================================

#ifndef __MATRIX4x4_H__
#define __MATRIX4x4_H__

#include <math.h>

/**
 * 4x4 matrix. All matrix elements (row, col) are indexed base 0.
 */
class Matrix4x4
{
public:
	/**
    * Constructor.  Sets the matrix to the identity matrix
    */
	Matrix4x4()
	{
      SetIdentity();
	}

   /**
    * Sets the matrix to the identity matrix.
    */
	void SetIdentity()
	{
		// Student to define
		a[0] = 1.f;
		a[1] = 0.f;
		a[2] = 0.f;
		a[3] = 0.f;
		a[4] = 0.f;
		a[5] = 1.f;
		a[6] = 0.f;
		a[7] = 0.f;
		a[8] = 0.f;
		a[9] = 0.f;
		a[10] = 1.f;
		a[11] = 0.f;
		a[12] = 0.f;
		a[13] = 0.f;
		a[14] = 0.f;
		a[15] = 1.f;
	}

	/**
    * Copy constructor
    * @param  n  Matrix to copy
    */
	Matrix4x4(const Matrix4x4& n)
	{
      // Student to define
		for(int i = 0; i < 16; i++)
		{
			a[i] = n.a[i];
		}
	}

   /**
    * Assignment operator
    * @param   n  Matrix to assign to this matrix
    * @return  Returns the address of this matrix.
    */
	Matrix4x4& operator = (const Matrix4x4& n)
	{
      // Student to define
		for(int i = 0; i < 16; i++)
		{
			a[i] = n.a[i];
		}
		return *this;
	}

   /**
    * Equality operator
    * @param   n  Matrix to test for equality with this matrix.
    * @return  Returns true if the matrices are equal, false otherwise..
    */
	bool operator == (const Matrix4x4& n) const
	{
      // Student to define
		for(int i = 0; i < 16; i++)
		{
			if(a[i] != n.a[i])
			{
				return false;
			}
		}

		return true;
	}

	/**
    * Destructor
    */
	~Matrix4x4() { }

   /**
    * Set the matrix to the values specified in the array.
    * @param  m  Array of float values to fill in this matrix. The
    *            elements are arranged in column order.
    */
   void Set(const float* m)
   {
		// Student to define
		for(int i = 0; i < 16; i++)
		{
			a[i] = m[i];
		}
   }

   /**
    * Gets the matrix (can be passed to OpenGL - GLSL mat4)
    * @return   Returns the elements of this matrix in column order.
    */
   const float* Get() const
   {
      // Student to define
      return a;
   }

   // Read-only access functions
   float m00() const { return a[0]; } // Student to define
	float m01() const { return a[4]; } // Student to define
	float m02() const { return a[8]; } // Student to define
	float m03() const { return a[12]; } // Student to define
	float m10() const { return a[1]; } // Student to define
	float m11() const { return a[5]; } // Student to define
	float m12() const { return a[9]; } // Student to define
	float m13() const { return a[13]; } // Student to define
	float m20() const { return a[2]; } // Student to define
	float m21() const { return a[6]; } // Student to define
	float m22() const { return a[10]; } // Student to define
	float m23() const { return a[14]; } // Student to define
	float m30() const { return a[3]; } // Student to define
	float m31() const { return a[7]; } // Student to define
	float m32() const { return a[11]; } // Student to define
	float m33() const { return a[15]; } // Student to define

   /**
    * Gets a matrix element given by row,column.
    * @param  row   Matrix row
    * @param  col   Matrix column
    * @return Returns the element at the specified row,col.
    */
   float m(const unsigned int row, const unsigned int col) const
   {
      // Student to define
      return a[row + col*4];
   }

   // Read-write access functions
   float& m00() { return a[0]; }// Student to define
   float& m01() { return a[4]; }// Student to define
   float& m02() { return a[8]; }// Student to define
   float& m03() { return a[12]; }// Student to define
   float& m10() { return a[1]; }// Student to define
   float& m11() { return a[5]; }// Student to define
   float& m12() { return a[9]; }// Student to define
	float& m13() { return a[13]; }// Student to define
	float& m20() { return a[2]; }// Student to define
	float& m21() { return a[6]; }// Student to define
	float& m22() { return a[10]; }// Student to define
	float& m23() { return a[14]; }// Student to define
	float& m30() { return a[3]; }// Student to define
	float& m31() { return a[7]; }// Student to define
	float& m32() { return a[11]; }// Student to define
	float& m33() { return a[15]; }// Student to define 

   /**
    * Gets a matrix element given by row,column. NOTE: this method takes 
    * row, col as 0 based elements (so it works with the Inverse method)
    * while the individual matrix access methods use a 1-based indexing.
    * @param  row   Matrix row (0-based)
    * @param  col   Matrix col (0-based)
    * @return Returns the address of the element at the specified row,col.
    */
   float& m(const unsigned int row, const unsigned int col)
   {
      // Student to define
      return a[row + col*4];
   }

   /**
    * Matrix multiplication.  Multiplies the current matrix by the matrix n
    * ( m' = m n ) and returns the resulting 4x4 matrix.
    * @param   n   Matrix to multiply the current matrix by
    * @return  Returns the product of the current matrix and the supplied matrix.
    */
   Matrix4x4 operator * (const Matrix4x4& n) const
   {
      // Unroll the loop, do 1 row at a time.
      Matrix4x4 t;
      float a0 = m00();
      float a1 = m01();
      float a2 = m02();
      float a3 = m03();
      t.m00() = a0 * n.m00() + a1 * n.m10() + a2 * n.m20() + a3 * n.m30();
      t.m01() = a0 * n.m01() + a1 * n.m11() + a2 * n.m21() + a3 * n.m31();
      t.m02() = a0 * n.m02() + a1 * n.m12() + a2 * n.m22() + a3 * n.m32();
      t.m03() = a0 * n.m03() + a1 * n.m13() + a2 * n.m23() + a3 * n.m33();
      a0 = m10();
      a1 = m11();
      a2 = m12();
      a3 = m13();
      t.m10() = a0 * n.m00() + a1 * n.m10() + a2 * n.m20() + a3 * n.m30();
      t.m11() = a0 * n.m01() + a1 * n.m11() + a2 * n.m21() + a3 * n.m31();
      t.m12() = a0 * n.m02() + a1 * n.m12() + a2 * n.m22() + a3 * n.m32();
      t.m13() = a0 * n.m03() + a1 * n.m13() + a2 * n.m23() + a3 * n.m33();
      a0 = m20();
      a1 = m21();
      a2 = m22();
      a3 = m23();
      t.m20() = a0 * n.m00() + a1 * n.m10() + a2 * n.m20() + a3 * n.m30();
      t.m21() = a0 * n.m01() + a1 * n.m11() + a2 * n.m21() + a3 * n.m31();
      t.m22() = a0 * n.m02() + a1 * n.m12() + a2 * n.m22() + a3 * n.m32();
      t.m23() = a0 * n.m03() + a1 * n.m13() + a2 * n.m23() + a3 * n.m33();
      a0 = m30();
      a1 = m31();
      a2 = m32();
      a3 = m33();
      t.m30() = a0 * n.m00() + a1 * n.m10() + a2 * n.m20() + a3 * n.m30();
      t.m31() = a0 * n.m01() + a1 * n.m11() + a2 * n.m21() + a3 * n.m31();
      t.m32() = a0 * n.m02() + a1 * n.m12() + a2 * n.m22() + a3 * n.m32();
      t.m33() = a0 * n.m03() + a1 * n.m13() + a2 * n.m23() + a3 * n.m33();
	   return t;
   }

   /**
    * Matrix multiplication.  Multiplies the current matrix by the matrix n
    * ( m = m n ) and stores the result in m.
    * @param   n  Matrix to multiply the current matrix by
    * @return  Returns the address of the current matrix.
    */
   Matrix4x4& operator *= (const Matrix4x4& n)
   {
		*this = *this * n;
		return *this;
   }

   /**
    * Matrix multiplication by a scalar.  Multiplies the current matrix 
    * by the scalar s.
    * @param  s   Scalar to multiply the current matrix by
    * @return     Returns the address of the current matrix.
    */
   Matrix4x4 & operator *= (const float s)
   {
	   // Student to define
	   for(int i = 0; i < 16; i++)
	   {
		   a[i] *= s;
	   }
	   return *this;
   }

   /**
    * Transforms a coordinate by the matrix.
    * @param   v  Homogeneous point.
    * @return  Returns the transformed homogeneous coordinate position.
    */
   HPoint3 operator *(const HPoint3& v) const
   {
		// Student to define
		HPoint3 pt;
		pt.x = m(0,0) * v.x + m(0,1) * v.y + m(0,2) + v.z + m(0,3) * v.w;
		pt.y = m(1,0) * v.x + m(1,1) * v.y + m(1,2) + v.z + m(1,3) * v.w;
		pt.z = m(2,0) * v.x + m(2,1) * v.y + m(2,2) + v.z + m(2,3) * v.w;
		pt.w = m(3,0) * v.x + m(3,1) * v.y + m(3,2) + v.z + m(3,3) * v.w;
		return pt;
   }

   /**
    * Transforms a coordinate by the matrix.  Returns a homogeneous
    * coordinate position.  Assumes the w coordinate is 1.
    * @param   v  3D point to transform.
    * @return  Returns the transformed point.
    */
   HPoint3 operator *(const Point3& v) const
   {
		// Student to define
		HPoint3 pt;
		pt.x = m(0,0) * v.x + m(0,1) * v.y + m(0,2) + v.z + m(0,3);
		pt.y = m(1,0) * v.x + m(1,1) * v.y + m(1,2) + v.z + m(1,3);
		pt.z = m(2,0) * v.x + m(2,1) * v.y + m(2,2) + v.z + m(2,3);
		pt.w = m(3,0) * v.x + m(3,1) * v.y + m(3,2) + v.z + m(3,3);
		return pt;
   }

   /**
    * Transforms a vector (normal or direction) by the matrix. 
    * Only the upper 3x3 portion of the matrix is used (no translation).
    * @param   v  3D vector to transform.
    * @return  Returns the transformed direction.
    */
   Vector3 operator *(const Vector3& v) const
   {
		// Student to define
		Vector3 rVec;
		rVec.x = m(0,0) * v.x + m(0,1) * v.y + m(0,2) + v.z;
		rVec.y = m(1,0) * v.x + m(1,1) * v.y + m(1,2) + v.z;
		rVec.z = m(2,0) * v.x + m(2,1) * v.y + m(2,2) + v.z;
		return rVec;
   }

   /**
    * Transposes the current matrix.
    * @return   Returns the address of the current matrix.
    */
   Matrix4x4& Transpose()
   {
		// Student to define
		
		Matrix4x4 tmp(*this);
		m(0,1) = tmp.m(1,0);
		m(0,2) = tmp.m(2,0);
		m(0,3) = tmp.m(3,0);
		m(1,0) = tmp.m(0,1);
		m(1,2) = tmp.m(2,1);
		m(1,3) = tmp.m(3,1);
		m(2,0) = tmp.m(0,2);
		m(2,1) = tmp.m(1,2);
		m(2,3) = tmp.m(3,2);
		m(3,0) = tmp.m(0,3);
		m(3,1) = tmp.m(1,3);
		m(3,2) = tmp.m(2,3);
		return *this;
   }

   /**
    * Calculates the transpose of the current 4x4 matrix and returns it.
    * The current matrix is unchanged.
    * @return   Returns the transpose of the current matrix.
    */
   Matrix4x4 GetTranspose() const
   {
	   // Student to define
      Matrix4x4 t(*this);
	  t.Transpose();
      return t;
   }

   // The following convenience methods allow creation of a composite 
   // modeling transfomation. Each method postmultiplies the curent
   // matrix (similar to OpenGL)

   /**
    * Applies a translation to the current transformation matrix.
    * Instead of doing a standard matrix multiplication call, we just reset
    * the elements of the matrix that will change due to the translation.
	 * @param	x	   x translation
	 * @param	y	   y translation
	 *	@param	z	   Z translation
	 */
	void Translate(const float x, const float y, const float z)
   {
      // Student to define
	   Matrix4x4 translate;
	   translate.SetIdentity();
	   translate.m(3,0) = x;
	   translate.m(3,1) = y;
	   translate.m(3,2) = z;
	   translate.m(3,3) = 1;

	   operator*=(translate);
   }

   /**
    * Applies a scaling to the current transformation matrix.
	 * @param	x	   x scaling
	 * @param	y	   y scaling
	 *	@param	z	   Z scaling
	 */
	void Scale(const float x, const float y, const float z)
   {
     	// Student to define
	   Matrix4x4 tmp;
	   tmp.m(0,0) = x;
	   tmp.m(1,1) = y;
	   tmp.m(2,2) = z;

	   operator*=(tmp);
   }

   /**
    * Performs a counterclockwise rotation about the specified axis. 
    * @param   angle    Angle (degrees) for the rotation.
    * @param   x        x coordinate of the axis of rotation
    * @param   y        y coordinate of the axis of rotation
    * @param   z        z coordinate of the axis of rotation
    */
   void Rotate(const float angle, const float x, const float y, 
               const float z)
   {
      // Student to define
	   Matrix4x4 tmp;
	   tmp.m(0,0) = cos(angle) + x*x * (1 - cos(angle));
	   tmp.m(0,1) = x*y * (1-cos(angle)) - z*sin(angle);
	   tmp.m(0,2) = x*z * (1-cos(angle)) + y*sin(angle);
	   tmp.m(1,0) = y*x * (1-cos(angle)) + z*sin(angle);
	   tmp.m(1,1) = cos(angle) + y*y * (1-cos(angle));
	   tmp.m(1,2) = y*z*(1-cos(angle)) - x*sin(angle);
	   tmp.m(2,0) = z*x*(1-cos(angle)) - y*sin(angle);
	   tmp.m(2,1) = z*y*(1-cos(angle)) + x*sin(angle);
	   tmp.m(2,2) = cos(angle) + z*z * (1-cos(angle));

	   operator*=(tmp);
   }

   /**
    * Calculates the inverse of the current 4x4 matrix and returns it.
    * @return  Returns the inverse of the current matrix.
    */
   Matrix4x4 GetInverse() const
   {
	   int j, k;
	   int ind;
	   float v1, v2;
	   Matrix4x4 t = *this;
      Matrix4x4 b;
	   for (int i = 0; i < 4; i++)
	   {
         // Find pivot
		   v1 = t.m(i, i);
		   ind = i;
		   for (j = i + 1; j < 4; j++)
		   {
			   if (fabs(t.m(j, i)) > fabs(v1))
			   {
				   ind = j;
				   v1 = t.m(j, i);
			   }
		   }

         // Swap columns
		   if (ind != i)
		   {
			   for (j = 0; j < 4; j++)
			   {
				   v2 = b.m(i, j);
				   b.m(i, j) = b.m(ind, j);
				   b.m(ind, j) = v2;
				   v2 = t.m(i, j);
				   t.m(i, j) = t.m(ind, j);
				   t.m(ind, j) = v2;
			   }
		   }

		   // The matrix is singular (has no inverse), set the inverse
		   //    to the identity matrix.
		   if (v1 == 0.0f)
		   {
            extern void logmsg(const char *message, ...);
			   logmsg("InvertMatrix: Singular matrix");
            b.SetIdentity();
			   return b;
		   }

		   for (j = 0; j < 4; j++)
		   {
			   t.m(i, j) /= v1;
			   b.m(i, j) /= v1;
		   }

         // Eliminate column
		   for (j = 0; j < 4; j++)
		   {
 			   if (j == i)
				   continue;

			   v1 = t.m(j, i);
			   for (k = 0; k < 4; k++)
			   {
				   t.m(j, k) -= t.m(i, k) * v1;
				   b.m(j, k) -= b.m(i, k) * v1;
			   }
		   }
	   }
	   return b;
   }

   /**
    * Logs a message followed by the matrix.
    *
    * @param   str   String to print to log file
    */  
   void Log(const char* str) const
   {
      extern void logmsg(const char *message, ...);
	   logmsg("  %s", str);
	   logmsg("%.3f %.3f %.3f %.3f", m00(), m01(), m02(), m03());
	   logmsg("%.3f %.3f %.3f %.3f", m10(), m11(), m12(), m13());
	   logmsg("%.3f %.3f %.3f %.3f", m20(), m21(), m22(), m23());
	   logmsg("%.3f %.3f %.3f %.3f", m30(), m31(), m32(), m33());
   }

private:
   // Student to define internal storage method
   float a[16];
};

#endif
